#include "misc.h"
#include <math.h>

#define SWAPWORD(val) (((val&0x00FF)<<8)+((val&0xFF00)>>8))

extern byte Angle_Data[];

byte Sine_Data[] = {
0x00,0x00,0x00,0x06,0x00,0x0c,0x00,0x12,0x00,0x19,0x00,0x1f,0x00,0x25,0x00,
0x2b,0x00,0x31,0x00,0x38,0x00,0x3e,0x00,0x44,0x00,0x4a,0x00,0x50,0x00,0x56,
0x00,0x5c,0x00,0x61,0x00,0x67,0x00,0x6d,0x00,0x73,0x00,0x78,0x00,0x7e,0x00,
0x83,0x00,0x88,0x00,0x8e,0x00,0x93,0x00,0x98,0x00,0x9d,0x00,0xa2,0x00,0xa7,
0x00,0xab,0x00,0xb0,0x00,0xb5,0x00,0xb9,0x00,0xbd,0x00,0xc1,0x00,0xc5,0x00,
0xc9,0x00,0xcd,0x00,0xd1,0x00,0xd4,0x00,0xd8,0x00,0xdb,0x00,0xde,0x00,0xe1,
0x00,0xe4,0x00,0xe7,0x00,0xea,0x00,0xec,0x00,0xee,0x00,0xf1,0x00,0xf3,0x00,
0xf4,0x00,0xf6,0x00,0xf8,0x00,0xf9,0x00,0xfb,0x00,0xfc,0x00,0xfd,0x00,0xfe,
0x00,0xfe,0x00,0xff,0x00,0xff,0x00,0xff,0x01,0x00,0x00,0xff,0x00,0xff,0x00,
0xff,0x00,0xfe,0x00,0xfe,0x00,0xfd,0x00,0xfc,0x00,0xfb,0x00,0xf9,0x00,0xf8,
0x00,0xf6,0x00,0xf4,0x00,0xf3,0x00,0xf1,0x00,0xee,0x00,0xec,0x00,0xea,0x00,
0xe7,0x00,0xe4,0x00,0xe1,0x00,0xde,0x00,0xdb,0x00,0xd8,0x00,0xd4,0x00,0xd1,
0x00,0xcd,0x00,0xc9,0x00,0xc5,0x00,0xc1,0x00,0xbd,0x00,0xb9,0x00,0xb5,0x00,
0xb0,0x00,0xab,0x00,0xa7,0x00,0xa2,0x00,0x9d,0x00,0x98,0x00,0x93,0x00,0x8e,
0x00,0x88,0x00,0x83,0x00,0x7e,0x00,0x78,0x00,0x73,0x00,0x6d,0x00,0x67,0x00,
0x61,0x00,0x5c,0x00,0x56,0x00,0x50,0x00,0x4a,0x00,0x44,0x00,0x3e,0x00,0x38,
0x00,0x31,0x00,0x2b,0x00,0x25,0x00,0x1f,0x00,0x19,0x00,0x12,0x00,0x0c,0x00,
0x06,0x00,0x00,0xff,0xfa,0xff,0xf4,0xff,0xee,0xff,0xe7,0xff,0xe1,0xff,0xdb,
0xff,0xd5,0xff,0xcf,0xff,0xc8,0xff,0xc2,0xff,0xbc,0xff,0xb6,0xff,0xb0,0xff,
0xaa,0xff,0xa4,0xff,0x9f,0xff,0x99,0xff,0x93,0xff,0x8b,0xff,0x88,0xff,0x82,
0xff,0x7d,0xff,0x78,0xff,0x72,0xff,0x6d,0xff,0x68,0xff,0x63,0xff,0x5e,0xff,
0x59,0xff,0x55,0xff,0x50,0xff,0x4b,0xff,0x47,0xff,0x43,0xff,0x3f,0xff,0x3b,
0xff,0x37,0xff,0x33,0xff,0x2f,0xff,0x2c,0xff,0x28,0xff,0x25,0xff,0x22,0xff,
0x1f,0xff,0x1c,0xff,0x19,0xff,0x16,0xff,0x14,0xff,0x12,0xff,0x0f,0xff,0x0d,
0xff,0x0c,0xff,0x0a,0xff,0x08,0xff,0x07,0xff,0x05,0xff,0x04,0xff,0x03,0xff,
0x02,0xff,0x02,0xff,0x01,0xff,0x01,0xff,0x01,0xff,0x00,0xff,0x01,0xff,0x01,
0xff,0x01,0xff,0x02,0xff,0x02,0xff,0x03,0xff,0x04,0xff,0x05,0xff,0x07,0xff,
0x08,0xff,0x0a,0xff,0x0c,0xff,0x0d,0xff,0x0f,0xff,0x12,0xff,0x14,0xff,0x16,
0xff,0x19,0xff,0x1c,0xff,0x1f,0xff,0x22,0xff,0x25,0xff,0x28,0xff,0x2c,0xff,
0x2f,0xff,0x33,0xff,0x37,0xff,0x3b,0xff,0x3f,0xff,0x43,0xff,0x47,0xff,0x4b,
0xff,0x50,0xff,0x55,0xff,0x59,0xff,0x5e,0xff,0x63,0xff,0x68,0xff,0x6d,0xff,
0x72,0xff,0x78,0xff,0x7d,0xff,0x82,0xff,0x88,0xff,0x8b,0xff,0x93,0xff,0x99,
0xff,0x9f,0xff,0xa4,0xff,0xaa,0xff,0xb0,0xff,0xb6,0xff,0xbc,0xff,0xc2,0xff,
0xc8,0xff,0xcf,0xff,0xd5,0xff,0xdb,0xff,0xe1,0xff,0xe7,0xff,0xee,0xff,0xf4,
0xff,0xfa,0x00,0x00,0x00,0x06,0x00,0x0c,0x00,0x12,0x00,0x19,0x00,0x1f,0x00,
0x25,0x00,0x2b,0x00,0x31,0x00,0x38,0x00,0x3e,0x00,0x44,0x00,0x4a,0x00,0x50,
0x00,0x56,0x00,0x5c,0x00,0x61,0x00,0x67,0x00,0x6d,0x00,0x73,0x00,0x78,0x00,
0x7e,0x00,0x83,0x00,0x88,0x00,0x8e,0x00,0x93,0x00,0x98,0x00,0x9d,0x00,0xa2,
0x00,0xa7,0x00,0xab,0x00,0xb0,0x00,0xb5,0x00,0xb9,0x00,0xbd,0x00,0xc1,0x00,
0xc5,0x00,0xc9,0x00,0xcd,0x00,0xd1,0x00,0xd4,0x00,0xd8,0x00,0xdb,0x00,0xde,
0x00,0xe1,0x00,0xe4,0x00,0xe7,0x00,0xea,0x00,0xec,0x00,0xee,0x00,0xf1,0x00,
0xf3,0x00,0xf4,0x00,0xf6,0x00,0xf8,0x00,0xf9,0x00,0xfb,0x00,0xfc,0x00,0xfd,
0x00,0xfe,0x00,0xfe,0x00,0xff,0x00,0xff,0x00,0xff
};

word CalcAngle(signed short xdist, signed short ydist)
{
//; ---------------------------------------------------------------------------
//; Subroutine calculate an angle

//; input:
//;	d1 = x-axis distance
//;	d2 = y-axis distance

//; output:
//;	d0 = angle
//; ---------------------------------------------------------------------------

//; ||||||||||||||| S U B	R O U T	I N E |||||||||||||||||||||||||||||||||||||||

    signed short angle; 

    //CalcAngle:
    //movem.l	d3-d4,-(sp)
    //moveq	#0,d3
    //moveq	#0,d4
    //move.w	d1,d3
    signed short _xdist = xdist;

    //move.w	d2,d4
    signed short _ydist = ydist;

    //or.w	d3,d4
    //beq.s	loc_2D04
    if ((_xdist | _ydist) != 0)
    {
        //move.w	d2,d4
	//tst.w	d3
        //bpl.w	loc_2CC2
	//neg.w	d3
        if (_xdist < 0) { _xdist = -_xdist; }

        //loc_2CC2:
	//tst.w	d4
	//bpl.w	loc_2CCA
	//neg.w	d4
        if (_ydist < 0) { _ydist = -ydist; }

        //loc_2CCA:
	//cmp.w	d3,d4
        //bcc.w	loc_2CDC
        if (_ydist < _xdist)
        {
             //lsl.l	#8,d4
            long ydistScale = _ydist * 256;             //these 2 lines are equivalent to using the distance formula
	   
            //divu.w	d3,d4
            word angleIndex = ydistScale / _xdist; //i.e. (ydist/xdist)*256, (256 corresponding angle entrys)

	    //moveq	#0,d0
	    //move.b	Angle_Data(pc,d4.w),d0
            angle = Angle_Data[angleIndex];

            //bra.s	loc_2CE6
        }
        else
        {
            //loc_2CDC:
	    //lsl.l	#8,d3
            long xdistScale = _xdist * 256;

	    //divu.w	d4,d3
            word angleIndex = xdistScale / _ydist;

	    //moveq	#$40,d0
	    //sub.b	Angle_Data(pc,d3.w),d0
            angle = Angle_Data[angleIndex] - 0x40;
        }

        //loc_2CE6:
	//tst.w	d1
        //bpl.w	loc_2CF2
	//neg.w	d0
        //addi.w	#$80,d0
        if (xdist > 0) { angle = -angle + 0x80; }

	//loc_2CF2:
	//tst.w	d2
	//bpl.w	loc_2CFE
	//neg.w	d0
	//addi.w	#$100,d0
        if (ydist > 0) { angle = -angle + 0x100; }

        //loc_2CFE:
	//movem.l	(sp)+,d3-d4
	//rts	
    }
    //; ===========================================================================
    else
    {
        //loc_2D04:
        //move.w	#$40,d0
        angle = 0x40;
	
        //movem.l	(sp)+,d3-d4
	//rts
    }
    return angle; 	
    
}    //; End of function CalcAngle

void CalcSine(word angle, signed short &sine, signed short &cosine)
{
     angle=((angle&0xFF)*2);
     word cosval = (*(word*)(&Sine_Data[angle+0x80]));
     word sinval = (*(word*)(&Sine_Data[angle]));
     

     cosine=(signed short)SWAPWORD(cosval);
     sine=(signed short)SWAPWORD(sinval);
     //cosine=(cos(((angle)*1.40625)*0.0174532925)*256);
     //sine=(sin(((angle)*1.40625)*0.0174532925)*256);
}

     
